/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 *
 */
public class Stager {

    private static SpeedController m_jaguarVertical = null;
    private static SpeedController m_jaguarHorizontal = null;

    public static boolean init() {
        m_jaguarVertical = Motor.getInstance(Motor.ID_STAGER_VERTICAL);
        m_jaguarHorizontal = Motor.getInstance(Motor.ID_STAGER_HORIZONTAL);
        if((m_jaguarVertical == null) || (m_jaguarHorizontal == null)) {
            Dashboard.setStagerStatus("ERR");
            return false;
        }
        return true;

    }

    /*
     * set the speed of the stager
     * speed can be between 0 and 1 inclusive
     */
    public static void setSpeed(double speed)
    {
        m_jaguarVertical.set(speed);
        m_jaguarHorizontal.set(speed);
    }

    /*
     * This routine returns true if the lifter is running, false otherwise
     */
    public static boolean isRunning()
    {
        // return true if the speed is non-zero
        // NOTE: Doing an equality comparison with a floating point number
        // is risky. Sometimes you can set a floating point variable to a number
        // and then when you read it back, you won't get exactly the same
        // number because of how the variable is stored in memory.
        // However, it should be safe to compare with a value of 0.
        return (m_jaguarVertical.get() != 0);
    }
}
